/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CScanMeasurementSet.h"
#include "BBSlammerWrapper.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "RGBImage.h"
#include "JPEGEncoder.hpp"  ///< For saving JPEG


static int enableGUI=1;
static int enableImageLogging=0;


void printUsage(){
	fprintf(stderr,"\n-----------------------------------------------------------------\n");
	fprintf(stderr,"Does SLAMMING\n");
	fprintf(stderr,"Usage: ./BBSlammerWrapperTester OPTS\n");
	fprintf(stderr,"\t[-P] name :: Uses 'name' as MaCI position file\n");
	fprintf(stderr,"\t[-R] name :: Uses 'name' as MaCI ranging file\n");
	fprintf(stderr,"\t[-L] name :: Uses 'name' as LnMlib measurement filename\n");
	fprintf(stderr,"\t[-S] name :: Uses 'name' as Settings file\n");
	fprintf(stderr,"\t[-M] name :: Uses 'name' as Global map file\n");
	fprintf(stderr,"\t[-d] value :: Uses 'value' as estimation distance\n");
	fprintf(stderr,"\nNote that -P and -R must both be given - -L is an alternative logfile format\n");
	fprintf(stderr,"\n'\n");
	fprintf(stderr,"-----------------------------------------------------------------\n\n");
}

void saveJPEG(int cnt, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		char fname[150];
		sprintf(fname,"%05d_image.jpg",cnt);
		fprintf(stderr,"Saving %s \n",fname); 
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);	
}



void timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Time elapsed (%.6fms = %.6fs) ",fms,fms/1000.0);
	}
}

int main(int argc, char *argv[]){
	debugInit();
	debugSetGlobalDebugLvl(4);
	char *measurement_file_name=NULL;
	char *position_file_name=NULL;
	char *ranging_file_name=NULL;
	char *settings_file_name = NULL;
	char *output_filename =  "MaCI_Position_matched.txt";
	char *map_file_name=NULL;
	float paramEstDistance = 10.0;
	////////////////////////////////////////////////////////////////////////////////////
	// Parse parameters
	////////////////////////////////////////////////////////////////////////////////////
	bool parse = true;
	while(parse) {
		int c;
		c = getopt(argc, argv, "P:R:L:S:M:d:hqv");
		switch(c) {
			
			/////////////////////////////////
			////// Application specific /////
			/////////////////////////////////
			case 'P': {
				// Take name & minor
				position_file_name = optarg;
				fprintf(stderr,"Using %s as Position file\n",position_file_name);
				break;
			}
			case 'M': {
				map_file_name = optarg;
				fprintf(stderr,"Using %s as Map file\n",map_file_name);
				break;
			}
			case 'R': {
				ranging_file_name = optarg;
				fprintf(stderr,"Using %s as Ranging file\n",ranging_file_name);
				break;
			}    
			case 'L':
				measurement_file_name = optarg;
				dPrint(3,"Using '%s' as LnMlib dataset filename", measurement_file_name);
				break;
			case 'd':
				paramEstDistance = atof(optarg);
				dPrint(3,"Using '%d' as estimation distance", paramEstDistance);
				break;
			case 'S':
				settings_file_name = optarg;
				dPrint(3,"Using '%s' as settings file name", settings_file_name);
				break;
			case 'h':
			case '?':
				printUsage();
				exit(1);
				break;
				
			case -1:
				parse = false;
				break;
		}
	}
	CBetBdtLocalizer *glocalizer; 
	glocalizer = new CBetBdtLocalizer(map_file_name);

	BBSlammerWrapper *BBS;
	gim::CSettings settings;
	int paramSetInitialPoseTo=0;
	float paramInitPoseX=0;
	float paramInitPoseY=0;
	float paramInitPoseA=0;
	if(settings_file_name==NULL){
		BBS = new BBSlammerWrapper(0.06, 60.0, ///Map 
																0.5,0.5,20.0*M_PI/180.0, ///Search 
																25.0, 0.02, 0.5, ///Fdist, minspan, pth 
																0.3,6.0*M_PI/180.0, ///Odo 
																0.9,12.0*M_PI/180.0,25.0, 20.0);
	}else{
		if(!settings.ReadSettingsXMLFile(settings_file_name)){
			dPrint(1,"Failed to read the configuration file!!!");
			return 0;
		}
		BBS = new BBSlammerWrapper(settings);
		
		settings.GetValue("Tester.enable_GUI", enableGUI);
		settings.GetValue("Tester.enable_image_logging",enableImageLogging);
		
		
		settings.GetValue("Tester.SetInitialPoseTo",paramSetInitialPoseTo);	
		settings.GetValue("Tester.InitPoseX",paramInitPoseX);	
		settings.GetValue("Tester.InitPoseY", paramInitPoseY);	
		settings.GetValue("Tester.InitPoseA", paramInitPoseA);	
	}
	////////////////////////////////////////////////////////////////////////////////////////

	CScanMeasurementSet *dset;
	if(position_file_name != NULL && ranging_file_name != NULL){
		dset = new CScanMeasurementSet(position_file_name,ranging_file_name);
	}else if(measurement_file_name != NULL){
		dset = new CScanMeasurementSet(measurement_file_name);
	} else{
		printUsage();
		fprintf(stderr,"\nERROR - You gotta give some measurement files for me!!! \n\n");
		exit(1);
	}
	fprintf(stderr,"DATASET size = %d Nummeas=%d\n", (int)dset->size(),dset->angles.size());
	
	
	
	int Nx,Ny;
	glocalizer->grid->getDim(Nx,Ny);
	RGBUtil::RGBImage *rgb;
	if(enableGUI || enableImageLogging){
		rgb = new RGBUtil::RGBImage(Nx,Ny);
	}
	if(enableGUI){
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
	}
	Grid::pose inip;
	if(paramSetInitialPoseTo){
			
			inip.set(paramInitPoseX,paramInitPoseY,paramInitPoseA);
			dset->convertToLaserFrame(inip);
			///BBS->setInitialPose(inip.x, inip.y, inip.a);
	}
	
	//////////////////////////////////////////////////////////////////////////////////////////
	///THIS DOES THE JOB
	//////////////////////////////////////////////////////////////////////////////////////////
	Grid::pose lp;
	Grid::TScan meas;
	
	for(int i=0;i<dset->size();i++){
		if(!dset->getMeasurementInLaserFrame(i, lp, meas)){
			fprintf(stderr,"FAILED to read measurement!!!!!\n");
			continue;
		}
		if(meas.N==0){
			fprintf(stderr,"No Measurement!!!!\n");
			continue;
		}
		timer(true);
		BBS->updateSimple(meas,lp);
		timer(false);
		
		float dist = BBS->pose_now.x*BBS->pose_now.x +  BBS->pose_now.y*BBS->pose_now.y;
		if(dist > paramEstDistance*paramEstDistance){ 
			fprintf(stderr,"ESTIMATION DISTANCE REACHED - Moving to estimation of initial pose\n");
			break;
		}
		
		if(enableImageLogging || enableGUI){
			rgb->set(BBS->slammer->grid->getDataPtr(), 255, 255, 255);
		}
		if(enableImageLogging) saveJPEG(i,rgb->getDataPtr(),Nx,Ny);
		if(enableGUI) displayImage(0, rgb->getDataPtr(),Ny,Nx);
		fprintf(stderr,"(%.2f,%.2f,%.2f)\n", BBS->pose_now.x,BBS->pose_now.y,BBS->pose_now.a );
	}
	
	float fdist = 100;
	float pTh = 0.4;
	float minSpan = 0.04;
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	float paramAx = 30.0;
	float paramAy = 30.0;
	float paramAa = M_PI;
	
	global_meas.frame.setError(paramAx,paramAy,paramAa);		
	global_meas.frame.setFrame(inip.x,inip.y,inip.a);
	global_meas.setOccupancyGrid(BBS->slammer->grid, 0.65);
	
	//////////////////////////////////////////////////////////
	fprintf(stderr,"Estimating intial pose............... \n");
	
	CMatchStorage matches(5000);
	glocalizer->localize(global_meas, matches);
	matches.sort();
	
	fprintf(stderr,"******************************************************************\n");
	fprintf(stderr,"RESULT = (%f %f %f) W=%f \n", matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight);
	
	fprintf(stderr,"******************************************************************\n");
	
	
	//glocalizer->localizeExt(global_meas,matches,numLeafs);
	
	/*
	//////////////////////////////////////////////////////////////////////////////////////////
	///SAVE THE RESULT
	//////////////////////////////////////////////////////////////////////////////////////////
	for(int i=0;i<BBS->all_poses.size();i++){
		dset->setMeasurementToRobotFrame(i, BBS->all_poses[i]);
	}
	dset->saveMaCIPositionFILE(output_filename,"Log","Matched");

	//////////////////////////////////////////////////////////////////////////////////////////
	///SAVE ALL INTO AN IMAGE
	//////////////////////////////////////////////////////////////////////////////////////////
	
	float paramMapSizeX=100;
	float paramMapSizeY=80;
	float paramMapResolution=0.05;
	TOccupancyGrid O(paramMapSizeX,paramMapSizeY,paramMapResolution,Grid::TCoord2D(0,0));
	O.setGridToValue(0.5);
	
	O.getDim(Nx,Ny);
	RGBUtil::RGBImage rgb_gl(Nx,Ny);
	for(int i=0;i<dset->size();i++){
		if(!dset->getMeasurementInLaserFrame(i, lp, meas)){
			fprintf(stderr,"FAILED TO READ\n");
			exit(1);
		}
		meas.filt(40.0);
		O.setScanFillProb(lp.x, lp.y ,lp.a,meas,0.3,0.99);
	}
	rgb_gl.set(O.getDataPtr(), 255, 255, 255);
						
	for(int i=0;i<dset->size();i++){
		Grid::TPixel pix = O.toPixel(Grid::TCoord2D(dset->poses[i].x,dset->poses[i].y));
		rgb_gl.setPixel(pix.x,pix.y, 255,0, 0 );
		dset->getPoseInLaserFrame(i,lp);
		pix = O.toPixel(Grid::TCoord2D(lp.x,lp.y));
		rgb_gl.setPixel(pix.x,pix.y, 0,255, 0 );
	}
	saveJPEG(dset->size()+2000,rgb_gl.getDataPtr(),Nx,Ny);
	BBS->slammer->grid->save("SLAM-Result.occu");
	*/
return 0;
}